#include "Auton.h"
#include "Arm.h"
#include "Roller.h"
#include "WPILib.h"

#define _GYRO_MULTIPLIER_ 0.05

Auton::Auton(RobotDrive* d, Arm* a, Roller* r, int encoderLA, int encoderLB, int encoderRA, int encoderRB, Gyro* g):
	encoderL(encoderLA, encoderLB),
	encoderR(encoderRA, encoderRB)
{
	drive = d;
	arm = a;
	roller = r;
	gyro = g;
	
	encoderL.SetDistancePerPulse(1);
	encoderR.SetDistancePerPulse(1);
	
	encoderL.Start();
	encoderR.Start();
	}

void Auton::Start() {
	int stage = 0;
	timer.Start();
	//timer.Reset();
	drive->TankDrive(0.0,0.0);
	drive->Drive(0.0,0.0);
	gyro->Reset();
	
	bool once = true;

	while(stage < 7) {
		switch (stage){
		case 0: 			//wrist down + arm up 
			if (timer.Get() < 0.2) {
				arm->Rotate(-1); //rotate up to half
			}
			else
			{
				arm->Deploy(); // deploy wrist
				timer.Reset();
				stage++;
			}
			break;
		case 1: 			//arm full up
			arm->Rotate(-1); // rotate the rest of the way
			if (timer.Get() > 1.5)
			{
				arm->Rotate(0);
				timer.Reset();
				stage++;
			}
			break;
		case 2: 			//drive 
			if(once)
			{
				//drive->TankDrive(0.0,0.0);
				drive->Drive(0.0,0.0);
				once = false;
				timer.Reset();
			}
			//drive->TankDrive(-0.6, -0.6);
			drive->Drive(-0.6, gyro->GetAngle()*_GYRO_MULTIPLIER_);
			if(timer.Get() > 4) {
				timer.Reset();
				stage++;  }
			break;	
		case 3:				//backup
			//drive->TankDrive(.5,.5);
			drive->Drive(.5, gyro->GetAngle() * _GYRO_MULTIPLIER_);
			if(timer.Get() < .5){
				stage++;	
				timer.Reset(); }		
			break;
		case 4: 			// arm down
		{
			arm->Rotate(.4); 
			if(timer.Get() > 0.5) {
				arm->Rotate(0.0);
				timer.Reset();
				stage++;  }
			break;
		}
		case 5: //score (then out)
			roller->Spin(-1.0); // out
			if(timer.Get() > 1.0) {
				timer.Reset();
				stage++;
			}
			break;
		case 6: //finish
			//arm->MoveTo(Arm::LOW);
			//drive->TankDrive(0.6, 0.6);      //back up
			drive->Drive(0.6, gyro->GetAngle() * _GYRO_MULTIPLIER_);
			if(timer.Get() > 1.0) {
				timer.Reset();
				stage++;
			}
			break;
		}
	}
}
